www.gusucode.com > VC 3D弹道仿真程序源码文件-源码程序 > VC 3D弹道仿真程序源码文件-源码程序/code/Target.cpp

    //Download by http://www.NewXing.com
#include "stdafx.h"

#include "TARGET.h"

CTARGET::CTARGET()
{
	Xc = 0.0 ; 	Yc = 0.0 ; 	Zc = 0.0;
	Vx = 0.0 ;	Vy = 0.0 ;	Vz = 0.0 ;
	Ax = 0.0 ;	Ay = 0.0 ;	Az = 0.0 ;
}

CTARGET::~CTARGET()
{
}

/*计算弹目视线角和视线角速度 函数*/
void CTARGET::Get_q( double XS[28], double dXS[28] ) 
{
	double dx, dy, dz, q[4];

	SetTarget( XS[3],XS[15] );

	dz = (XS[5] - Zc);
	dy = (XS[4] - Yc);
	dx = (XS[3] - Xc);

	q[0] = atan(dy/dx);                                 //纵向目标高低视线角 rad
    q[1] = (dx*dXS[4]-dXS[3]*dy)/(dx*dx+dy*dy);         //纵向目标高低视线角速度  rad/s

	dz = (XS[5] - Zc);
	dy = (XS[4] - Yc);
	dx = (XS[3] - Xc);

	q[2] = -atan(dz/dx);                                //横向目标方位角 rad
    q[3] = -(dx*dXS[5]-dXS[3]*dz)/(dx*dx+dz*dz);        //横向目标方位角速度  rad/s

	for ( int i = 0; i < 4 ; ++ i )
	{
		this->q[i] = q[i] ;
	}
	XS[24] = q[0] ;	XS[25] = q[1] ;	
	XS[26] = q[2] ;	XS[27] = q[3] ;

	return ;
}

/*设置目标 函数*/
void CTARGET::SetTarget( double x, double h )
{
	if ( (x>50000) &&(h < 15000.0 ) )	// 最终命中的真实目标
	{
		Xc     = 80000.0 ;				// 目标距投弹点水平距离坐标(m)
		Yc     = -503.17;				// 目标距地面y坐标(m)
		Zc     = 0.0;          
	}
	else								// 虚拟目标比例导引段的虚拟目标
	{
		Xc     = 35210.7;				// 目标距投弹点水平距离坐标(m)
		Yc     = 15000.0;				// 目标距地面y坐标(m)
		Zc     = 0.0;          	
	}
}